//
// Created by Sonny on 2023/10/4.
//
#include "UPRE_Drv_DjiMotor.h"
#define ABS(x)		((x>0)? x: -x)


void DjiMotor::Init( Enum_Control_Method _Control_Method,float _Gearbox_Rate,float _Torque_Max) {

    Control_Method = _Control_Method;
    Gearbox_Rate = _Gearbox_Rate;
    Torque_Max =   _Torque_Max;

}

void DjiMotor::Get_Moter_Info(CAN_HandleTypeDef *hcan,uint8_t RxBuff[8]) {
//    uint16_t msg_cnt;
//    msg_cnt++;
    int16_t delta_encoder;
    if(hcan->Instance == CAN1) {
        can.Moter_ID = RxHeader1.StdId - 0x200;
        can.TxHeader.StdId =   can.Moter_ID <= 4 ?can.S2M_ID1 : can.S2M_ID2;
    }
    if(hcan->Instance == CAN2) {
        can.Moter_ID = RxHeader2.StdId - 0x200;
        can.TxHeader.StdId =   can.Moter_ID <= 4 ?can.S2M_ID1 : can.S2M_ID2;
    }

    for (int i = 0; i < 8; i++) {
        can.Rx_Data[i] = RxBuff[i];
    }

    Pre_Encoder = Rx_Encoder;

    Rx_Encoder = (can.Rx_Data[0] << 8) | can.Rx_Data[1];
    Rx_Omega = (can.Rx_Data[2] << 8) | can.Rx_Data[3];
    Rx_Torque = (can.Rx_Data[4] << 8) | can.Rx_Data[5];
    Rx_Temperature = can.Rx_Data[6];

//    if (msg_cnt >= 10) {
        delta_encoder = Rx_Encoder - Pre_Encoder;
        if (delta_encoder < -4096)
        {
            //正方向转过了一圈
            Total_Round++;
        }
        else if (delta_encoder > 4096)
        {
            //反方向转过了一圈
            Total_Round--;
        }
        Total_Encoder = Total_Round * Encoder_Num_Per_Round + Rx_Encoder;

        Now_Angle = (float)Total_Encoder / (float)Encoder_Num_Per_Round * 360 / Gearbox_Rate;
        Now_Omega = (float)Rx_Omega;
        Now_Torque = Rx_Torque;
        Now_Temperature = Rx_Temperature;

//    }
}

void DjiMotor::Output() {
    if (can.TxHeader.StdId == can.S2M_ID1) {
        if (can.Moter_ID == 1) {
            can.Tx_Data[0] = (uint8_t) (OUT >> 8);
            can.Tx_Data[1] = (uint8_t) OUT;
        }
        else if (can.Moter_ID == 2) {
            can.Tx_Data[2] = (uint8_t)(OUT >>8);
            can.Tx_Data[3] = (uint8_t)OUT;
        }
        else if (can.Moter_ID == 3){
            can.Tx_Data[4] = (uint8_t)(OUT >>8);
            can.Tx_Data[5] = (uint8_t)OUT;
        }
        else if (can.Moter_ID == 4){
            can.Tx_Data[6] = (uint8_t)(OUT >>8);
            can.Tx_Data[7] = (uint8_t)OUT;
        }
    }
    else if (can.TxHeader.StdId == can.S2M_ID2) {
        if (can.Moter_ID == 5) {
            can.Tx_Data[0] = (uint8_t) (OUT >> 8);
            can.Tx_Data[1] = (uint8_t) OUT;
        }
        else if (can.Moter_ID == 6) {
            can.Tx_Data[2] = (uint8_t)(OUT >>8);
            can.Tx_Data[3] = (uint8_t)OUT;
        }
        else if (can.Moter_ID == 7){
            can.Tx_Data[4] = (uint8_t)(OUT >>8);
            can.Tx_Data[5] = (uint8_t)OUT;
        }
        else if (can.Moter_ID == 8){
            can.Tx_Data[6] = (uint8_t)(OUT >>8);
            can.Tx_Data[7] = (uint8_t)OUT;
        }
    }

    can.CAN_Send_Data();
}

void DjiMotor::Control_PID(){
    switch (Control_Method) {
        case Control_Method_OPENLOOP:{

            OUT = PID_Torque.OUt[0] / Torque_Output * Torque_Output;
        }
        break;
        case Control_Method_TORQUE:{
            PID_Torque.Target[0] = Target_Torque;
            PID_Torque.Now[0] = Now_Torque;
            PID_Torque.PID_Calc();

            OUT = PID_Torque.OUt[0];
        }
        break;
        case Control_Method_OMEGA:{
            PID_Omega.Target[0] = Target_Omega;
            PID_Omega.Now[0] = Now_Omega;
            PID_Omega.PID_Calc();

            OUT = PID_Omega.OUt[0];
        }
        break;
        case Control_Method_ANGLE:{
            PID_Angle.Target[0] = Target_Angle;
            PID_Angle.Now[0] = Now_Angle;
            PID_Angle.PID_Calc();

            OUT = PID_Angle.OUt[0];
        }
        break;
        case Control_Method_OMEGA_ANGLE:{
            PID_Angle.Target[0] = Target_Angle;
            PID_Angle.Now[0] = Now_Angle;
            PID_Angle.PID_Calc();

            Target_Omega = PID_Angle.OUt[0];

            PID_Omega.Target[0] = Target_Omega;
            PID_Omega.Now[0] = Now_Omega;
            PID_Omega.PID_Calc();

            OUT = PID_Omega.OUt[0];
        }
        break;
        case Control_Method_TEST: {
            if (PID_Test.Test_start == 1) {
                if (Test_cnt < 2000) {
                    OUT = 0;
                } else OUT = 400;
                Test_cnt++;
            }else{
                Test_cnt = 0;
                OUT = 0;

            }
        }
    }
    Output();
}

void DjiMotor::Set_TargetParam(float Target_Angle,float Target_Omega) {

    this->Target_Angle = Target_Angle;
    this->Target_Omega = Target_Omega;
}
